/*
 * KDTree.cpp
 *
 *  Created on: Apr 20, 2011
 *      Author: alki
 */

#include "KDTree.h"

#include <iostream>
#include <stdlib.h>

using namespace std;

KDTree::KDTree() {
	this->m_node1 = NULL;
	this->m_node2 = NULL;
//	this->m_objekte.clear();
	this->m_min = Vektor(0,0,0);
	this->m_max = Vektor(0,0,0);
}

KDTree::~KDTree() {
	if(this->m_node1!=NULL) {
		delete this->m_node1;
		this->m_node1 = NULL;
	}
	if(this->m_node2!=NULL) {
		delete this->m_node1;
		this->m_node2 = NULL;
	}
	this->m_objekte.clear();
	this->m_min = Vektor(0,0,0);
	this->m_max = Vektor(0,0,0);
}

void KDTree::insert(const Objekt* objekt) {
	this->m_objekte.push_back(objekt);
}

void KDTree::split(unsigned int max) {
	if(this->m_objekte.size()<=max) {
		return;
	}

	Vektor mid=this->m_objekte[0]->getPosition();

	for(unsigned int i=1;i<this->m_objekte.size();i++) {
		mid=mid+this->m_objekte[i]->getPosition();
	}

	mid=mid/this->m_objekte.size();

	Vektor med=(this->m_max+this->m_min)/2;

	float x=fabs(mid.getX()-med.getX());
	float y=fabs(mid.getY()-med.getY());
	float z=fabs(mid.getZ()-med.getZ());

	this->m_node1 = new KDTree();
	this->m_node2 = new KDTree();
	this->m_node1->m_max=this->m_max;
	this->m_node1->m_min=this->m_min;
	this->m_node2->m_max=this->m_max;
	this->m_node2->m_min=this->m_min;

	if(x>=y && x>=z) { // Nehme Y-Z-Ebene
		cout << "X" << endl;
		this->m_node1->m_max.setX(mid.getX());
		this->m_node2->m_min.setX(mid.getX());

		for(unsigned int i=0;i<this->m_objekte.size();i++) {
			if(this->m_node1->ObjInTree(this->m_objekte[i])) {
				this->m_node1->insert(this->m_objekte[i]);
			}
			else {
				this->m_node2->insert(this->m_objekte[i]);
			}
		}
	}
	else if(y>=x && y>=z) { // Nehme X-Z-Ebene
		cout << "Y" << endl;
		this->m_node1->m_max.setY(mid.getY());
		this->m_node2->m_min.setY(mid.getY());

		for(unsigned int i=0;i<this->m_objekte.size();i++) {
			if(this->m_node1->ObjInTree(this->m_objekte[i])) {
				this->m_node1->insert(this->m_objekte[i]);
			}
			else {
				this->m_node2->insert(this->m_objekte[i]);
			}
		}
	}
	else { // Nehme X-Y-Ebene
		cout << "Z" << endl;
		this->m_node1->m_max.setZ(mid.getZ());
		this->m_node2->m_min.setZ(mid.getZ());

		for(unsigned int i=0;i<this->m_objekte.size();i++) {
			if(this->m_node1->ObjInTree(this->m_objekte[i])) {
				this->m_node1->insert(this->m_objekte[i]);
			}
			else {
				this->m_node2->insert(this->m_objekte[i]);
			}
		}
	}

	this->m_node1->resize();
	this->m_node2->resize();
	this->m_node1->clear();
	this->m_node2->clear();

	for(unsigned int i=0;i<this->m_objekte.size();i++) {
		if(this->m_node1->Intersects(this->m_objekte[i])) {
			this->m_node1->insert(this->m_objekte[i]);
		}
		if(this->m_node2->Intersects(this->m_objekte[i])) {
			this->m_node2->insert(this->m_objekte[i]);
		}
	}

	if((*(this->m_node1)<this && this->m_node1->m_objekte.size()>max)) {
		this->m_node1->split(max);
	}

	if((*(this->m_node2)<this && this->m_node2->m_objekte.size()>max)) {
		this->m_node2->split(max);
	}

	if(this->m_node1->m_objekte.size()==this->m_node2->m_objekte.size() && this->m_node1->m_objekte.size()==this->m_objekte.size()) {
		delete this->m_node1;
		delete this->m_node2;
		this->m_node1=NULL;
		this->m_node2=NULL;
	}
}

bool KDTree::operator<(const KDTree *tree) const {
	if(this->m_objekte.size()>=tree->m_objekte.size())
		return false;

	if(this->m_min<tree->m_min)
		return false;

	if(this->m_max>tree->m_max)
		return false;

	return true;
}

bool KDTree::Intersects(const Objekt *objekt) const {
	Vektor max=objekt->getMax();
	Vektor min=objekt->getMin();

	Vektor mid1=(this->m_max+this->m_min)/2;
	Vektor mid2=(max+min)/2;
	Vektor range1=(this->m_max-this->m_min)/2;
	Vektor range2=(max-min)/2;

	if(fabs((mid1-mid2).getX())>fabs((range1+range2).getX())) return false;
	if(fabs((mid1-mid2).getY())>fabs((range1+range2).getY())) return false;
	if(fabs((mid1-mid2).getZ())>fabs((range1+range2).getZ())) return false;

	return true;
}

bool KDTree::ObjInTree(const Objekt *objekt) const {
	if(this->m_min.getX()>objekt->getPosition().getX()) return false;
	if(this->m_max.getX()<objekt->getPosition().getX()) return false;
	if(this->m_min.getY()>objekt->getPosition().getY()) return false;
	if(this->m_max.getY()<objekt->getPosition().getY()) return false;
	if(this->m_min.getZ()>objekt->getPosition().getZ()) return false;
	if(this->m_max.getZ()<objekt->getPosition().getZ()) return false;

	return true;
}

void KDTree::out() const {
	cout << "Min: " << this->m_min.getX() << " | " << this->m_min.getY() << " | " << this->m_min.getZ() << endl;
	cout << "Max: " << this->m_max.getX() << " | " << this->m_max.getY() << " | " << this->m_max.getZ() << endl;
	cout << "Obj: " << this->m_objekte.size() << endl;
}

void KDTree::out(int ebenen) const {
	for(int i=ebenen;i>0;i--) {
		for(int j=0;j<(pow(2,i-1)-1);j++) cout << " ";

		this->out(i,ebenen);cout << endl;
	}
}

void KDTree::out(int ebene,int cursor) const {
	if(cursor>ebene) {
		if(this->m_node1!=NULL)
			this->m_node1->out(ebene,cursor-1);
		else cout << "X";

		for(int i=0;i<(pow(2,ebene)-1);i++) cout << " ";

		if(this->m_node2!=NULL)
			this->m_node2->out(ebene,cursor-1);
		else cout << "X";
	}
	if(cursor==ebene) {
		cout << this->m_objekte.size();
	}
}

void KDTree::leafesout() const {
	if(this->m_node1!=NULL) {
		this->m_node1->leafesout();
		this->m_node2->leafesout();
	}
	else {
		for(unsigned int i=0;i<this->m_objekte.size();i++)
			cout << this->m_objekte[i]->index << ",";
	}
}

void KDTree::resize() {
	for(unsigned int i=0;i<this->m_objekte.size();i++) {
		Vektor min,max;
		min=this->m_objekte[i]->getMin();
		max=this->m_objekte[i]->getMax();

		if(i==0) {
			this->m_min=min;
			this->m_max=max;
			continue;
		}

		if(this->m_min.getX()>min.getX())
			this->m_min.setX(min.getX());
		if(this->m_min.getY()>min.getY())
			this->m_min.setY(min.getY());
		if(this->m_min.getZ()>min.getZ())
			this->m_min.setZ(min.getZ());
		if(this->m_max.getX()<max.getX())
			this->m_max.setX(max.getX());
		if(this->m_max.getY()<max.getY())
			this->m_max.setY(max.getY());
		if(this->m_max.getZ()<max.getZ())
			this->m_max.setZ(max.getZ());
	}

//	cout << endl << endl;

//	this->m_min.out();
//	this->m_max.out();
}

float KDTree::Intersects(const Gerade &ray) const {
	Vektor diff=this->getMid()-ray.getPos();
	if(diff.ScalarProduct(ray.getDir())<=-0.0001)
		return -1;

	float ret=(((this->m_max+this->m_min)/2)-ray.getPos()).Length();

	typedef struct aabb {
		float x0,y0,z0;
		float x1,y1,z1;
	} AABB;

	aabb b;
	b.x0=this->m_min.getX();
	b.y0=this->m_min.getY();
	b.z0=this->m_min.getZ();
	b.x1=this->m_max.getX();
	b.y1=this->m_max.getY();
	b.z1=this->m_max.getZ();

    switch (ray.classification)
    {
	case MMM:

		if ((ray.getPos().getX() < b.x0) || (ray.getPos().getY() < b.y0) || (ray.getPos().getZ() < b.z0)
			|| (ray.jbyi * b.x0 - b.y1 + ray.c_xy > 0)
			|| (ray.ibyj * b.y0 - b.x1 + ray.c_yx > 0)
			|| (ray.jbyk * b.z0 - b.y1 + ray.c_zy > 0)
			|| (ray.kbyj * b.y0 - b.z1 + ray.c_yz > 0)
			|| (ray.kbyi * b.x0 - b.z1 + ray.c_xz > 0)
			|| (ray.ibyk * b.z0 - b.x1 + ray.c_zx > 0)
			)
			return -1;

		return ret;

	case MMP:

		if ((ray.getPos().getX() < b.x0) || (ray.getPos().getY() < b.y0) || (ray.getPos().getZ() > b.z1)
			|| (ray.jbyi * b.x0 - b.y1 + ray.c_xy > 0)
			|| (ray.ibyj * b.y0 - b.x1 + ray.c_yx > 0)
			|| (ray.jbyk * b.z1 - b.y1 + ray.c_zy > 0)
			|| (ray.kbyj * b.y0 - b.z0 + ray.c_yz < 0)
			|| (ray.kbyi * b.x0 - b.z0 + ray.c_xz < 0)
			|| (ray.ibyk * b.z1 - b.x1 + ray.c_zx > 0)
			)
			return -1;

		return ret;

	case MPM:

		if ((ray.getPos().getX() < b.x0) || (ray.getPos().getY() > b.y1) || (ray.getPos().getZ() < b.z0)
			|| (ray.jbyi * b.x0 - b.y0 + ray.c_xy < 0)
			|| (ray.ibyj * b.y1 - b.x1 + ray.c_yx > 0)
			|| (ray.jbyk * b.z0 - b.y0 + ray.c_zy < 0)
			|| (ray.kbyj * b.y1 - b.z1 + ray.c_yz > 0)
			|| (ray.kbyi * b.x0 - b.z1 + ray.c_xz > 0)
			|| (ray.ibyk * b.z0 - b.x1 + ray.c_zx > 0)
			)
			return -1;

		return ret;

	case MPP:

		if ((ray.getPos().getX() < b.x0) || (ray.getPos().getY() > b.y1) || (ray.getPos().getZ() > b.z1)
			|| (ray.jbyi * b.x0 - b.y0 + ray.c_xy < 0)
			|| (ray.ibyj * b.y1 - b.x1 + ray.c_yx > 0)
			|| (ray.jbyk * b.z1 - b.y0 + ray.c_zy < 0)
			|| (ray.kbyj * b.y1 - b.z0 + ray.c_yz < 0)
			|| (ray.kbyi * b.x0 - b.z0 + ray.c_xz < 0)
			|| (ray.ibyk * b.z1 - b.x1 + ray.c_zx > 0)
			)
			return -1;

		return ret;

	case PMM:

		if ((ray.getPos().getX() > b.x1) || (ray.getPos().getY() < b.y0) || (ray.getPos().getZ() < b.z0)
			|| (ray.jbyi * b.x1 - b.y1 + ray.c_xy > 0)
			|| (ray.ibyj * b.y0 - b.x0 + ray.c_yx < 0)
			|| (ray.jbyk * b.z0 - b.y1 + ray.c_zy > 0)
			|| (ray.kbyj * b.y0 - b.z1 + ray.c_yz > 0)
			|| (ray.kbyi * b.x1 - b.z1 + ray.c_xz > 0)
			|| (ray.ibyk * b.z0 - b.x0 + ray.c_zx < 0)
			)
			return -1;

		return ret;

	case PMP:

		if ((ray.getPos().getX() > b.x1) || (ray.getPos().getY() < b.y0) || (ray.getPos().getZ() > b.z1)
			|| (ray.jbyi * b.x1 - b.y1 + ray.c_xy > 0)
			|| (ray.ibyj * b.y0 - b.x0 + ray.c_yx < 0)
			|| (ray.jbyk * b.z1 - b.y1 + ray.c_zy > 0)
			|| (ray.kbyj * b.y0 - b.z0 + ray.c_yz < 0)
			|| (ray.kbyi * b.x1 - b.z0 + ray.c_xz < 0)
			|| (ray.ibyk * b.z1 - b.x0 + ray.c_zx < 0)
			)
			return -1;

		return ret;

	case PPM:

		if ((ray.getPos().getX() > b.x1) || (ray.getPos().getY() > b.y1) || (ray.getPos().getZ() < b.z0)
			|| (ray.jbyi * b.x1 - b.y0 + ray.c_xy < 0)
			|| (ray.ibyj * b.y1 - b.x0 + ray.c_yx < 0)
			|| (ray.jbyk * b.z0 - b.y0 + ray.c_zy < 0)
			|| (ray.kbyj * b.y1 - b.z1 + ray.c_yz > 0)
			|| (ray.kbyi * b.x1 - b.z1 + ray.c_xz > 0)
			|| (ray.ibyk * b.z0 - b.x0 + ray.c_zx < 0)
			)
			return -1;

		return ret;

	case PPP:

		if ((ray.getPos().getX() > b.x1) || (ray.getPos().getY() > b.y1) || (ray.getPos().getZ() > b.z1)
			|| (ray.jbyi * b.x1 - b.y0 + ray.c_xy < 0)
			|| (ray.ibyj * b.y1 - b.x0 + ray.c_yx < 0)
			|| (ray.jbyk * b.z1 - b.y0 + ray.c_zy < 0)
			|| (ray.kbyj * b.y1 - b.z0 + ray.c_yz < 0)
			|| (ray.kbyi * b.x1 - b.z0 + ray.c_xz < 0)
			|| (ray.ibyk * b.z1 - b.x0 + ray.c_zx < 0)
			)
			return -1;

		return ret;

	case OMM:

		if((ray.getPos().getX() < b.x0) || (ray.getPos().getX() > b.x1)
			|| (ray.getPos().getY() < b.y0) || (ray.getPos().getZ() < b.z0)
			|| (ray.jbyk * b.z0 - b.y1 + ray.c_zy > 0)
			|| (ray.kbyj * b.y0 - b.z1 + ray.c_yz > 0)
			)
			return -1;

		return ret;

	case OMP:

		if((ray.getPos().getX() < b.x0) || (ray.getPos().getX() > b.x1)
			|| (ray.getPos().getY() < b.y0) || (ray.getPos().getZ() > b.z1)
			|| (ray.jbyk * b.z1 - b.y1 + ray.c_zy > 0)
			|| (ray.kbyj * b.y0 - b.z0 + ray.c_yz < 0)
			)
			return -1;

		return ret;

	case OPM:

		if((ray.getPos().getX() < b.x0) || (ray.getPos().getX() > b.x1)
			|| (ray.getPos().getY() > b.y1) || (ray.getPos().getZ() < b.z0)
			|| (ray.jbyk * b.z0 - b.y0 + ray.c_zy < 0)
			|| (ray.kbyj * b.y1 - b.z1 + ray.c_yz > 0)
			)
			return -1;

		return ret;

	case OPP:

		if((ray.getPos().getX() < b.x0) || (ray.getPos().getX() > b.x1)
			|| (ray.getPos().getY() > b.y1) || (ray.getPos().getZ() > b.z1)
			|| (ray.jbyk * b.z1 - b.y0 + ray.c_zy < 0)
			|| (ray.kbyj * b.y1 - b.z0 + ray.c_yz < 0)
			)
			return -1;

		return ret;

	case MOM:

		if((ray.getPos().getY() < b.y0) || (ray.getPos().getY() > b.y1)
			|| (ray.getPos().getX() < b.x0) || (ray.getPos().getZ() < b.z0)
			|| (ray.kbyi * b.x0 - b.z1 + ray.c_xz > 0)
			|| (ray.ibyk * b.z0 - b.x1 + ray.c_zx > 0)
			)
			return -1;

		return ret;

	case MOP:

		if((ray.getPos().getY() < b.y0) || (ray.getPos().getY() > b.y1)
			|| (ray.getPos().getX() < b.x0) || (ray.getPos().getZ() > b.z1)
			|| (ray.kbyi * b.x0 - b.z0 + ray.c_xz < 0)
			|| (ray.ibyk * b.z1 - b.x1 + ray.c_zx > 0)
			)
			return -1;

		return ret;

	case POM:

		if((ray.getPos().getY() < b.y0) || (ray.getPos().getY() > b.y1)
			|| (ray.getPos().getX() > b.x1) || (ray.getPos().getZ() < b.z0)
			|| (ray.kbyi * b.x1 - b.z1 + ray.c_xz > 0)
			|| (ray.ibyk * b.z0 - b.x0 + ray.c_zx < 0)
			)
			return -1;

		return ret;

	case POP:

		if((ray.getPos().getY() < b.y0) || (ray.getPos().getY() > b.y1)
			|| (ray.getPos().getX() > b.x1) || (ray.getPos().getZ() > b.z1)
			|| (ray.kbyi * b.x1 - b.z0 + ray.c_xz < 0)
			|| (ray.ibyk * b.z1 - b.x0 + ray.c_zx < 0)
			)
			return -1;

		return ret;

	case MMO:

		if((ray.getPos().getZ() < b.z0) || (ray.getPos().getZ() > b.z1)
			|| (ray.getPos().getX() < b.x0) || (ray.getPos().getY() < b.y0)
			|| (ray.jbyi * b.x0 - b.y1 + ray.c_xy > 0)
			|| (ray.ibyj * b.y0 - b.x1 + ray.c_yx > 0)
			)
			return -1;

		return ret;

	case MPO:

		if((ray.getPos().getZ() < b.z0) || (ray.getPos().getZ() > b.z1)
			|| (ray.getPos().getX() < b.x0) || (ray.getPos().getY() > b.y1)
			|| (ray.jbyi * b.x0 - b.y0 + ray.c_xy < 0)
			|| (ray.ibyj * b.y1 - b.x1 + ray.c_yx > 0)
			)
			return -1;

		return ret;

	case PMO:

		if((ray.getPos().getZ() < b.z0) || (ray.getPos().getZ() > b.z1)
			|| (ray.getPos().getX() > b.x1) || (ray.getPos().getY() < b.y0)
			|| (ray.jbyi * b.x1 - b.y1 + ray.c_xy > 0)
			|| (ray.ibyj * b.y0 - b.x0 + ray.c_yx < 0)
			)
			return -1;

		return ret;

	case PPO:

		if((ray.getPos().getZ() < b.z0) || (ray.getPos().getZ() > b.z1)
			|| (ray.getPos().getX() > b.x1) || (ray.getPos().getY() > b.y1)
			|| (ray.jbyi * b.x1 - b.y0 + ray.c_xy < 0)
			|| (ray.ibyj * b.y1 - b.x0 + ray.c_yx < 0)
			)
			return -1;

		return ret;

	case MOO:

		if((ray.getPos().getX() < b.x0)
			|| (ray.getPos().getY() < b.y0) || (ray.getPos().getY() > b.y1)
			|| (ray.getPos().getZ() < b.z0) || (ray.getPos().getZ() > b.z1)
			)
			return -1;

		return ret;

	case POO:

		if((ray.getPos().getX() > b.x1)
			|| (ray.getPos().getY() < b.y0) || (ray.getPos().getY() > b.y1)
			|| (ray.getPos().getZ() < b.z0) || (ray.getPos().getZ() > b.z1)
			)
			return -1;

		return ret;

	case OMO:

		if((ray.getPos().getY() < b.y0)
			|| (ray.getPos().getX() < b.x0) || (ray.getPos().getX() > b.x1)
			|| (ray.getPos().getZ() < b.z0) || (ray.getPos().getZ() > b.z1)
			)
			return -1;

	case OPO:

		if((ray.getPos().getY() > b.y1)
			|| (ray.getPos().getX() < b.x0) || (ray.getPos().getX() > b.x1)
			|| (ray.getPos().getZ() < b.z0) || (ray.getPos().getZ() > b.z1)
			)
			return -1;

	case OOM:

		if((ray.getPos().getZ() < b.z0)
			|| (ray.getPos().getX() < b.x0) || (ray.getPos().getX() > b.x1)
			|| (ray.getPos().getY() < b.y0) || (ray.getPos().getY() > b.y1)
			)
			return -1;

	case OOP:

		if((ray.getPos().getZ() > b.z1)
			|| (ray.getPos().getX() < b.x0) || (ray.getPos().getX() > b.x1)
			|| (ray.getPos().getY() < b.y0) || (ray.getPos().getY() > b.y1)
			)
			return -1;

		return ret;

	}

	return -1;
}

void KDTree::getObjekte(vector<const Objekt*> *objekte) const {
	for(unsigned int i=0;i<this->m_objekte.size();i++) {
		objekte->push_back(this->m_objekte[i]);
	}
}
